
#include"kalman_filter.h"

double KalmanFilter(Kalman* kalman, double Z)
{
	double x = kalman->A * kalman->X + kalman->B * kalman->U;   

	kalman->P = kalman->A  * kalman->P * kalman->A + kalman->Q;

	kalman->K = kalman->P * kalman->H / (kalman->H * kalman->P * kalman->H + kalman->R);

	kalman->X = x + (Z - kalman->H * x) * kalman->K;

	kalman->P = (1 - kalman->K * kalman->H) * kalman->P;

	return  kalman->X;
} 
